#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static bool _compare_points_same(const std::vector<cv::Point>& pts1, const std::vector<cv::Point>& pts2) {
    if (pts1.size() != pts2.size()) {
        return false;
    }
    for (int i = 0; i < pts1.size(); i++) {
        if (pts1[i].x != pts2[i].x || pts1[i].y != pts2[i].y) {
            return false;
        }
    }
    return true;
}

static void _push_line_index(std::vector<std::pair<int, int>>& lineIndexStack, int& stackSize, int& top, std::pair<int, int> lineIndex) {
    if (top >= stackSize) {
        lineIndexStack.resize(stackSize*3/2);
        stackSize = lineIndexStack.size();
    }
    lineIndexStack[top] = lineIndex;
    top++;
}

static std::vector<cv::Point> _approx_poly_Douglas_Peuker(const std::vector<cv::Point>& pts, double eps, bool closed) {
    if (eps < 0.0 || !(eps < 1e30)) {
        //throw std::exception::exception("Epsilon not valid!");
        std::cout << "Epsilon not valie!" << std::endl;
        std::vector<cv::Point> v;
        return v;
    }
    
    std::vector<cv::Point> approxCurve;
    int count = pts.size();
    if (count == 0) {
        return approxCurve;
    }
    
    eps *= eps;
    
    std::pair<int, int> lineIndex = std::make_pair(0, 0);
    std::pair<int, int> otherLineIndex = std::make_pair(0, 0);
    cv::Point lineFirstPt(0, 0), lineSecondPt(0, 0), pt(0, 0);
    bool isLessEqualThanEps = false;
    int isClosed = closed;
    
    std::vector<std::pair<int, int>> lineIndexStack(count);
    int top = 0;
    int stackSize = lineIndexStack.size();
    
    if (!isClosed) {
        lineSecondPt = pts[0];
        lineFirstPt = pts[count-1];
        if (lineSecondPt.x != lineFirstPt.x || lineSecondPt.y != lineFirstPt.y) {
            _push_line_index(lineIndexStack, stackSize, top, std::make_pair(0, count-1));            
        } else {
            isClosed = 1;
        }
    }
    
    if (isClosed) {
        double dist, max_dist = 0;
        lineFirstPt = pts[0];
        for (int j = 1; j < count; j++) {
            lineSecondPt = pts[j];
            double dx = lineSecondPt.x - lineFirstPt.x;
            double dy = lineSecondPt.y - lineFirstPt.y;
            dist = dx * dx + dy * dy;
            if (dist > max_dist) {
                max_dist = dist;
                otherLineIndex.first = j;
            }
        }
        isLessEqualThanEps = (max_dist <= eps);
        
        if (!isLessEqualThanEps) {
            lineIndex.first = 0;
            otherLineIndex.second = lineIndex.first;
            lineIndex.second = otherLineIndex.first;
            _push_line_index(lineIndexStack, stackSize, top, otherLineIndex);
            _push_line_index(lineIndexStack, stackSize, top, lineIndex);
        } else {
            approxCurve.push_back(lineFirstPt);
        }
    }
    
    while (top > 0) {
        top--;
        lineIndex = lineIndexStack[top];
        lineSecondPt = pts[lineIndex.second];
        lineFirstPt = pts[lineIndex.first];
        if ((lineIndex.first+1) % count != lineIndex.second) {
            double dx, dy, dist, max_dist = 0;
            dx = lineSecondPt.x - lineFirstPt.x;
            dy = lineSecondPt.y - lineFirstPt.y;
            for (int j = (lineIndex.first+1) % count; j != lineIndex.second; j = (j+1) % count) {
                pt = pts[j];
                dist = std::fabs((pt.y-lineFirstPt.y)*dx - (pt.x-lineFirstPt.x)*dy);
                if (dist > max_dist) {
                    max_dist = dist;
                    otherLineIndex.first = (j+count)%count;
                }
            }
            isLessEqualThanEps = (max_dist * max_dist <= eps * (dx*dx+dy*dy));
        } else {
            isLessEqualThanEps = true;
        }
        
        if (isLessEqualThanEps) {
            approxCurve.push_back(lineFirstPt);
        } else {
            otherLineIndex.second = lineIndex.second;
            lineIndex.second = otherLineIndex.first;
            _push_line_index(lineIndexStack, stackSize, top, otherLineIndex);
            _push_line_index(lineIndexStack, stackSize, top, lineIndex);
        }
    }
    
    if (!isClosed) {
        approxCurve.push_back(pts[count-1]);
    }
    
    return approxCurve;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::Mat::zeros(cv::Size(400, 400), CV_8UC1);
    cv::rectangle(src, cv::Rect(50, 50, 300, 300), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::line(src, cv::Point(40, 150), cv::Point(80, 160), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(src, cv::Point(40, 300), cv::Point(110, 280), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(src, cv::Point(150, 40), cv::Point(170, 90), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(src, cv::Point(310, 40), cv::Point(290, 100), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(src, cv::Point(330, 240), cv::Point(360, 235), cv::Scalar::all(0), 10, cv::LINE_8);
    std::vector<std::vector<cv::Point>> vvpts;
    std::vector<cv::Point> polygon;
    polygon.push_back(cv::Point(160, 360));
    polygon.push_back(cv::Point(200, 280));
    polygon.push_back(cv::Point(240, 360));
    vvpts.push_back(polygon);
    cv::fillPoly(src, vvpts, cv::Scalar::all(0));
    
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(src, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    
    //double eps = 0.01 * cv::arcLength(contours[0], true);
    double eps = 0.001 * cv::arcLength(contours[0], true);
    std::vector<cv::Point> approxCurve, approxCurve1;
    bool closed = false;
    cv::approxPolyDP(contours[0], approxCurve, eps, closed);
    approxCurve1 = _approx_poly_Douglas_Peuker(contours[0], eps, closed);
    
    cv::Mat res = cv::Mat::zeros(cv::Size(400, 400), CV_8UC3);
    cv::rectangle(res, cv::Rect(50, 50, 300, 300), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::line(res, cv::Point(40, 150), cv::Point(80, 160), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(res, cv::Point(40, 300), cv::Point(110, 280), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(res, cv::Point(150, 40), cv::Point(170, 90), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(res, cv::Point(310, 40), cv::Point(290, 100), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::line(res, cv::Point(330, 240), cv::Point(360, 235), cv::Scalar::all(0), 10, cv::LINE_8);
    cv::fillPoly(res, vvpts, cv::Scalar::all(0));
    cv::Mat res1 = res.clone();
    for (int i = 0; i < approxCurve.size()-1; i++) {
        cv::line(res, approxCurve[i], approxCurve[i+1], cv::Scalar(0, 0, 255), 5);
    }
    for (int i = 0; i < approxCurve1.size()-1; i++) {
        cv::line(res1, approxCurve1[i], approxCurve1[i+1], cv::Scalar(255, 0, 0), 5);
    }
    
    std::cout << "results are " << (_compare_points_same(approxCurve, approxCurve1) ? "same" : "not same") << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("res", res);
    cv::imshow("res1", res1);
    cv::waitKey(0);
    return 0;
}
